#include "elevator_contorller.h"

int sockfd; // our socket file descriptor
int sockfd_ui;

RT_TASK communications,
        status,
        floor,
        supervisor,
        ui_stuff;

RT_ALARM supervisor_alarm,
         status_alarm,
         floor_alarm;

RT_MUTEX buffer_mutex[2],
         current_info_mutex,
         next_floor_mutex,
         flying_solo_mutex,
         next_floor_mutex;

int cb = 0;
system_packet current_info;
system_backet buffer[2];
int flying_solo = 0;
int next_floor = 0;


int compare (const void * a, const void * b)
{
    return ( *(int*)b - *(int*)a );
}

void resort(){
    rt_mutex_acquire( &current_info_mutex, TM_INFINTE );
    qsort( current_info.floors_q, NUN_OF_FLOORS, sizeof(int), compare );
    rt_mutex_acquire( &current_info_mutex );

}

void add_floor_to_q( int i ){
    int j;
    int s = 0;
    rt_mutex_acquire( &current_info_mutex, TM_INFINITE );
    for( j = 0; j < NUM_OF_FLOORS; j++ ){
        if( current_info.floors_q[j] == i ){
            rt_mutex_release( &current_info_mutex );
            return;
        }
    }

    for( j = NUM_OF_FLOORS - 1; j >= 0; j-- ){
        if( current_info.floors_q[j] == 0 ){
            current_info.floors_q[j] = i;
            s = 1;
            break;
        }
    }

    rt_mutex_release( &current_info_mutex );
    if( s ) { resort(); }


}

void remove_floor_from_q( int i ){
    int j;
    int s = 0;
    rt_mutex_acquire( &current_info_mutex, TM_INFINITE );
    for( j = 0; j < NUM_OF_FLOORS; j++ ){
        if( current_info.floors_q[j] == i ){
            current_info.floors_q[j] = 0;
            s = 1;
            break;
        }
    }
    rt_mutex_release( &current_info_mutex );
    if( s ) { resort(); }

}

void set_next_floor(){
    rt_mutex_acquire( &current_info_mutex, TM_INFINITE );
    system_packet temp = current_info;
    rt_mutex_release( &current_info_mutex );
    int j;
    rt_mutex_acquire( &next_floor_mutex, TM_INFINTE );
    if( temp.direction == UP ){
        for( j = NUM_OF_FLOORS - 1; j >= 0; j-- ){
            if( temp.floors_q[j] > temp.current_floor ){
                next_floor = temp.floors_q[j];
                break;
            }
        }
    }
    }else if( temp.direction == DOWN ){
        for( j = 0; j < NUM_OF_FLOORS; j++){
            if( temp.floors_q[j] < temp.current_floor ){
                next_floor = temp.floors_q[j];
                break;
        }
    }else{
        next_floor = temp.floors_q[0];
    }

    rt_mutex_release( & next_floor_mutex );
}

void communications_run( void *arg){
    struct sockaddr_in serveraddr;
	struct hostent *server = NULL;
	char *hostname = "localhost";
	char buf[BUFSIZE] = {0};

	sockfd = socket(AF_INET, SOCK_STREAM, 0);
	if( !sockfd ){
		error("Could not open a socket");
	}		
	server = gethostbyname(hostname);
	if(server == NULL){
		error("Could not resolve host name");
	}
	
	bzero((char *) &serveraddr, sizeof(serveraddr));
	serveraddr.sin_family = AF_INET;
	bcopy((char *)server->h_addr,
		(char *)&serveraddr.sin_addr.s_addr, server->h_length);
	serveraddr.sin_port = htons(PORT);

	if ( connect(sockfd,(struct sockaddr *) &serveraddr, sizeof(serveraddr)) < 0 ){
		error("Error connecting");
	}
    flying_solo = 1;

    //Communicate with server here, have yet to establish a protocol
    while( 1 ){
        rt_task_sleep( 100 * MS );
    }

}

void status_run( void *arg){
    while(1){
        int err = rt_alarm_wait( &status_alarm );
        if( !err ){
            system_packet temp;
            rt_mutex_acquire( &current_info_mutex, TM_INFINITE );
            temp = current_info;
            rt_mutex_release( &current_info_mutex );
            int temps;
            rt_mutex_acquire( &buffer[cb], TM_INFINITE);
            temps = cb
            buffer[cb] = temp;
            cb = (cb + 1) % 2;
            rt_mutex_release( &buffer[temps] );
        }
    }
}

void floor_run( void *arg){
    while(1){
        int err = rt_alarm_wait( &floor_alarm );
        if( ! err ){
            rt_mutex_acquire(&next_floor_mutex, TM_INFINITE);
            int tnf =  next_floor;
            rt_mutex_release(&next_floor_mutex);
            rt_mutex_acquire( &current_info_mutex, TM_INFINTE);
            int tcf = current_info.current_floor;
            rt_mutex_release( &current_info_mutex );
            if( tcf == tnf )
            {
                rt_alarm_stop( &floor_alarm ):
                rt_printf("Opening door.\n");
                rt_task_sleep( DOOR_OPEN_TIME );
                rt_printf("Waiting for button press.\n");
                rt_task_sleep( DOOR_AJAR_TIME );
                int i = 0;
                read( sockfd_ui, &i, sizeof(int) );
                rt_printf("Closing door.\n");
                rt_task_sleep( DOOR_CLOSE_TIME );

                if( i > 0 ){
                    add_floor_to_q( i );
                }
            
                remove_floor_from_q( current_info.current_floor );

                rt_alarm_start( &floor_alarm, SEC_PER_FLOOR , SEC_PER_FLOOR);
            }

            rt_mutex_acquire( &current_info_mutex, TM_INFINITE );
            if( current_info.direction == UP ){
                if( current_info.current_floor < NUM_OF_FLOORS ){
                    current_info.current_floor++;
                    
                }
            }
            else if( current_info.direction == DOWN ){
                if( current_info.current_floor > STARTING_FLOOR ){
                    current_info.current_floor--;
                }
            }
            if( current_info.current_floor == NUM_OF_FLOORS  || current_info.current_floor == STARTING_FLOOR){
                        current_info.direction = NA;
            }
            rt_mutex_release( &current_info_mutex );
        }
}
void supervisor_run( void *arg){
    while(1){
        if( flying_solo ){
            //Do stuff that commincations is supposed to do
        }
        rt_task_sleep( SUPER_SLEEP );
    }
}

void finish(int sig){
	puts("\nRecieved interrupt");
    
    close( sockfd );
    close( sockfd_ui );
	exit(EXIT_SUCCESS);
}

void error( char *msg) {
	perror(msg);
	close(sockfd);
    close(sockfd_ui);
	exit(EXIT_FAILURE);
}



int set_non_blocking(int fd)
{
    int flags;

    /* If they have O_NONBLOCK, use the Posix way to do it */
    #if defined(O_NONBLOCK)
    /* Fixme: O_NONBLOCK is defined but broken on SunOS 4.1.x and AIX 3.2.5. */
    if (-1 == (flags = fcntl(fd, F_GETFL, 0)))
        flags = 0;
    return fcntl(fd, F_SETFL, flags | O_NONBLOCK);
    #else
    /* Otherwise, use the old way of doing it */
    flags = 1;
    return ioctl(fd, FIOBIO, &flags);
    #endif
}

int main(void){
	signal(SIGINT, finish);
    mlockall(MCL_CURRENT|MCL_FUTURE);
	
    if( rt_task_create(&communications, NULL, 0, 99, T_JOINABLE) )
    {
        error("Could not create task");
    }
    if( rt_task_create(&floor, NULL, 0, 99, 0) )
    {
        error("Could not create task");
    }
    if( rt_task_create(&status, NULL, 0, 99, T_JOINABLE) )
    {
        error("Could not create task");
    }
    if( rt_task_create(&supervisor, NULL, 0, 99, T_JOINABLE) )
    {
        error("Could not create task");
    }
    if( rt_mutex_create( &buffer_mutex[0], NULL ) ){
        error("Could not create mutex");
    }
    if( rt_mutex_create( &buffer_mutex[1], NULL ) ){
        error("Could not create mutex");
    }

    current_info.current_floor = STARTING_FLOOR;
    current_info.direction = NA;
    curernt_info.floors_q = {0};

    if( rt_task_run( &communications, &communications_run, NULL )){
        error("Could not start task");
    }

    if( rt_task_run( &status, &status_run, NULL )){
        error("Could not start task");
    }

    if( rt_task_run( &supervisor, &supervisor_run, NULL )){
        error("Could not start task");
    }

    rt_task_join(&communications);
    rt_task_join(&status);
    rt_task_join(&supervisor);

    return EXIT_SUCCESS;
}
